

#include "BVHIndex.h"
#include "math.h"
#include <algorithm>
#include "AABBTri.h"

using namespace BVHIndex;

//////////////////////////////////////////////////////////////////////////
//CVoxel
// void CVoxel::SetOccupied(bool bFlag)
// {
// 	bOccupied = bFlag;
// }
// 
// void CVoxel::SetBBox(BoundingBox bbox)
// {
// 	BBox = bbox;
// }
// 
// bool CVoxel::GetOccupied()
// {
// 	return bOccupied;
// }
// 
// BoundingBox CVoxel::GetBBox()
// {
// 	return BBox;
// }
// 
// Point3d CVoxel::GetCenter(double dVoxelSize)
// {
// 	BoundingBox bbox = GetBBox();
// 	Point3d	ptMax = Point3d(bbox.Max.x * dVoxelSize, bbox.Max.y * dVoxelSize, bbox.Max.z * dVoxelSize);
// 	Point3d ptMin = Point3d(bbox.Min.x * dVoxelSize, bbox.Min.y * dVoxelSize, bbox.Min.z * dVoxelSize);
// 	return Point3d((ptMax + ptMin).x / 2.0, (ptMax + ptMin).x / 2.0, (ptMax + ptMin).x / 2.0);
// }

//////////////////////////////////////////////////////////////////////////
//boundingbox
// BoundingBox::BoundingBox(Pointlist pts, double dVoxel)
// {
// 	//calculate the bbox of pts and make it regular bbox based on dVoxel
// 	Pointlist::iterator pt_itr = pts.begin();
// 	Vector3d vecMax = *pt_itr[0];
// 	Vector3d vecMin = *pt_itr++[0];
// 	for (; pt_itr != pts.end(); pt_itr++)
// 	{
// 		if ((*pt_itr)->x > vecMax.x)
// 			vecMax.x = (*pt_itr)->x;
// 		if ((*pt_itr)->y > vecMax.y)
// 			vecMax.y = (*pt_itr)->y;
// 		if ((*pt_itr)->z > vecMax.z)
// 			vecMax.z = (*pt_itr)->z;
// 
// 		if ((*pt_itr)->x < vecMin.x)
// 			vecMin.x = (*pt_itr)->x;
// 		if ((*pt_itr)->y < vecMin.y)
// 			vecMin.y = (*pt_itr)->y;
// 		if ((*pt_itr)->z < vecMin.z)
// 			vecMin.z = (*pt_itr)->z;
// 	}
// 	Vector3d vLength = vecMax - vecMin;
// 
// 	//regularized size
// 	Indice3i iVoxelSize;
// 	iVoxelSize.x = 2^static_cast<int>(log(vLength.x/dVoxel)/log(2.0) + 0.5);
// 	iVoxelSize.y = 2^static_cast<int>(log(vLength.y/dVoxel)/log(2.0) + 0.5);
// 	iVoxelSize.z = 2^static_cast<int>(log(vLength.z/dVoxel)/log(2.0) + 0.5);
// 
// 	Min = Indice3i(0, 0, 0);
// 	Max = Min + iVoxelSize;
// }
// 
// Indice3i BoundingBox::GetSize()
// {
// 	return Indice3i(Max.x - Min.x, Max.y - Min.y, Max.z - Min.z);
// }
// 
// int BoundingBox::GetMaxSize()
// {
// 	return max(max(Max.x - Min.x, Max.y - Min.y), Max.z - Min.z);
// }

//////////////////////////////////////////////////////////////////////////
//CBVHVoxelIndex
/*
CBVHVoxelIndex::CBVHVoxelIndex()
{
	children.clear();
}

ptr_BVHNode CBVHVoxelIndex::ConstructChildren(BoundingBox bbox, ptr_BVHNode parent)
{
	Indice3i iSize = bbox.GetSize();

	if (iSize.x * iSize.y * iSize.z == 1)
	{
		//create the leaf node
		ptr_BVHNode leafNode;
		leafNode->parent = parent;
		leafNode->BBox = bbox;
		leafNode->VoxelData = make_shared<CVoxel>(false, bbox);
		leafNode->children.clear();

		return leafNode;
	}
	else
	{
		//create the node
		ptr_BVHNode Node;
		Node->parent = parent;
		Node->BBox = bbox;
		Node->VoxelData = nullptr;

		//create left and right children
		BoundingBox leftBBox, rightBBox;
		leftBBox.Min = bbox.Min;
		rightBBox.Max = bbox.Max;

		Indice3i iHalfSize(iSize.x/2, iSize.y/2, iSize.z/2);
		if (iHalfSize.x >= max(iHalfSize.z, iHalfSize.y))
		{
			//split along yz
			leftBBox.Max = Indice3i(bbox.Min.x + iHalfSize.x, bbox.Max.y, bbox.Max.z);
			rightBBox.Min = Indice3i(bbox.Min.x + iHalfSize.x, bbox.Min.y, bbox.Min.z);
		}
		else if(iHalfSize.y >= max(iHalfSize.z, iHalfSize.x))
		{
			//split along xz
			leftBBox.Max = Indice3i(bbox.Max.x, bbox.Min.y + iHalfSize.y, bbox.Max.z);
			rightBBox.Min = Indice3i(bbox.Min.x, bbox.Min.y + iHalfSize.y, bbox.Min.z);
		}
		else
		{
			//split along xy
			leftBBox.Max = Indice3i(bbox.Max.x, bbox.Max.y, bbox.Min.z + iHalfSize.z);
			rightBBox.Min = Indice3i(bbox.Min.x, bbox.Min.y, bbox.Min.z + iHalfSize.z);
		}
 
		Node->children.push_back(ConstructChildren(leftBBox, Node));
		Node->children.push_back(ConstructChildren(rightBBox, Node));
		return Node;
	}
}

//construct BVH and voxels based on a set of point3d
CBVHVoxelIndex::CBVHVoxelIndex(Pointlist pts, double dVoxel)
{
	//calculate the regular bbox of pts
	dVoxelSize = dVoxel;
	BoundingBox ptsbbox(pts, dVoxel);
	Indice3i iLength = ptsbbox.GetSize();

	//construct index recursively
	parent = nullptr;
	BBox.Max = ptsbbox.Max;
	BBox.Min = ptsbbox.Min;

	if (iLength.x * iLength.y * iLength.z > 1 )
	{
		VoxelData = nullptr;
		ConstructChildren(ptsbbox, ptr_BVHNode(this));
	}
	else if(iLength.x * iLength.y * iLength.z == 1 )
	{
		VoxelData = make_shared<CVoxel>(false, BBox);
	}
	else
	{
		return;
	}
}

CBVHVoxelIndex::~CBVHVoxelIndex()
{
	children.clear();
}

int CBVHVoxelIndex::CountDepth(ptr_BVHNode Node)
{
	if (Node->children.size() == 0)
	{
		return 0;
	}
	else
	{
		return 1 + max(CountDepth(Node->children[0]), CountDepth(Node->children[1]));
	}
}

int CBVHVoxelIndex::GetBVHDepth()
{
	ptr_BVHNode pRoot = GetRoot();
	return CountDepth(pRoot);
}

int CBVHVoxelIndex::GetNodeDepth(ptr_BVHNode Node)
{
	int iDepth = 0;

	ptr_BVHNode ptr = Node;
	while (ptr->parent != nullptr)
	{
		ptr = ptr->parent;
		iDepth++;
	}

	return iDepth;
}

ptr_BVHNode CBVHVoxelIndex::GetRoot()
{
	if (IsRoot())
	{
		return ptr_BVHNode(this);
	}
	else
	{
		ptr_BVHNode ptr = parent;
		while (!ptr->IsRoot())
		{
			ptr = ptr->parent;
		}

		return ptr;
	}
}

void CBVHVoxelIndex::GetVoxels(vector<ptr_CVoxel>& result)
{
	if (this->IsLeaf())
	{
		result.push_back(this->GetData());
	}
	else
	{
		if (this->children[0])
			this->children[0]->GetVoxels(result);
		if (this->children[1])
			this->children[1]->GetVoxels(result);
	}
}

//Query operations
ptr_CVoxel CBVHVoxelIndex::QuerybyPoint(Point3d pt)
{
	if (this->BBox.Min.x * dVoxelSize <= pt.x <= this->BBox.Max.x * dVoxelSize &&
		this->BBox.Min.y * dVoxelSize <= pt.y <= this->BBox.Max.y * dVoxelSize &&
		this->BBox.Min.z * dVoxelSize <= pt.z <= this->BBox.Max.z * dVoxelSize)
	{
		if (this->IsLeaf())
		{
			return this->GetData();
		}
		if (this->children[0])
			return this->children[0]->QuerybyPoint(pt);
		if (this->children[1])
			return this->children[1]->QuerybyPoint(pt);
	}
	else
		return nullptr;
}

void CBVHVoxelIndex::QuerybyBbox(Point3d ptMax, Point3d ptMin, vector<ptr_BVHNode> &result)
{
	if (ptMin.x <= this->BBox.Min.x * dVoxelSize && this->BBox.Max.x * dVoxelSize <= ptMax.x &&
		ptMin.y <= this->BBox.Min.y * dVoxelSize && this->BBox.Max.y * dVoxelSize <= ptMax.y &&
		ptMin.z <= this->BBox.Min.z * dVoxelSize && this->BBox.Max.z * dVoxelSize <= ptMax.z)
	{
		result.push_back(ptr_BVHNode(this));
	}
	else
	{
		if (this->children[0])
			this->children[0]->QuerybyBbox(ptMax, ptMin, result);
		if (this->children[1])
			this->children[1]->QuerybyBbox(ptMax, ptMin, result);
	}
}

vector<ptr_CVoxel> CBVHVoxelIndex::QuerybyBbox(Point3d ptMax, Point3d ptMin)
{
	vector<ptr_BVHNode> NodeResult;
	QuerybyBbox(ptMax, ptMin, NodeResult);
	vector<ptr_CVoxel> VoxelResult;
	vector<ptr_BVHNode>::iterator NodeItr = NodeResult.begin();
	for (;NodeItr != NodeResult.end(); NodeItr ++)
	{
		vector<ptr_CVoxel> TempVoxelResult;
		(*NodeItr)->GetVoxels(TempVoxelResult);
		vector<ptr_CVoxel>::iterator VoxelItr = TempVoxelResult.begin();
		for (;VoxelItr != TempVoxelResult.end(); VoxelItr ++)
		{
			VoxelResult.push_back(*VoxelItr);
		}
	}
	return VoxelResult;
}

vector<ptr_CVoxel> CBVHVoxelIndex::QuerybyShell(Polyhedron3 poly, Pointlist pts)
{
	vector<ptr_CVoxel> VoxelResult;
	
	return VoxelResult;
}

vector<ptr_CVoxel> CBVHVoxelIndex::QuerybySurface(Surface3 surface, Pointlist pts)
{
	vector<ptr_CVoxel> result;
	Trianglelist::iterator itrTri = surface.facets.begin();
	for (; itrTri != surface.facets.end(); itrTri++)
	{
		//
		vector<ptr_CVoxel> TempVoxelResult, VoxelResult;
		Point3d ptMax, ptMin;
		ptMax.x = max(max(pts[(*itrTri)->vert[0]]->x, pts[(*itrTri)->vert[2]]->x), pts[(*itrTri)->vert[3]]->x) + dVoxelSize;
		ptMax.y = max(max(pts[(*itrTri)->vert[0]]->y, pts[(*itrTri)->vert[2]]->y), pts[(*itrTri)->vert[3]]->y) + dVoxelSize;
		ptMax.z = max(max(pts[(*itrTri)->vert[0]]->z, pts[(*itrTri)->vert[2]]->z), pts[(*itrTri)->vert[3]]->z) + dVoxelSize;
		ptMin.x = min(min(pts[(*itrTri)->vert[0]]->x, pts[(*itrTri)->vert[2]]->x), pts[(*itrTri)->vert[3]]->x) - dVoxelSize;
		ptMin.y = min(min(pts[(*itrTri)->vert[0]]->y, pts[(*itrTri)->vert[2]]->y), pts[(*itrTri)->vert[3]]->y) - dVoxelSize;
		ptMin.z = min(min(pts[(*itrTri)->vert[0]]->z, pts[(*itrTri)->vert[2]]->z), pts[(*itrTri)->vert[3]]->z) - dVoxelSize;
		TempVoxelResult = QuerybyBbox(ptMax, ptMin);
		//
		vector<ptr_CVoxel>::iterator itrVoxel = TempVoxelResult.begin();
		for (; itrVoxel != TempVoxelResult.end(); itrVoxel++)
		{
			BoundingBox bbox = (*itrVoxel)->GetBBox();
			Point3d	ptMax = Point3d(bbox.Max.x * dVoxelSize, bbox.Max.y * dVoxelSize, bbox.Max.z * dVoxelSize);
			Point3d ptMin = Point3d(bbox.Min.x * dVoxelSize, bbox.Min.y * dVoxelSize, bbox.Min.z * dVoxelSize);
			float cent[3];
			float boxhalfsize[3];
			float triverts[3][3];
			cent[0] = (ptMax + ptMin).x / 2.0;
			cent[1] = (ptMax + ptMin).y / 2.0;
			cent[2] = (ptMax + ptMin).z / 2.0;
			boxhalfsize[0] = dVoxelSize / 2.0;
			boxhalfsize[1] = dVoxelSize / 2.0;
			boxhalfsize[2] = dVoxelSize / 2.0;
			triverts[0][0] = pts[(*itrTri)->vert[0]]->x;
			triverts[0][1] = pts[(*itrTri)->vert[0]]->y;
			triverts[0][2] = pts[(*itrTri)->vert[0]]->z;
			triverts[1][0] = pts[(*itrTri)->vert[1]]->x;
			triverts[1][1] = pts[(*itrTri)->vert[1]]->y;
			triverts[1][2] = pts[(*itrTri)->vert[1]]->z;
			triverts[2][0] = pts[(*itrTri)->vert[2]]->x;
			triverts[2][1] = pts[(*itrTri)->vert[2]]->y;
			triverts[2][2] = pts[(*itrTri)->vert[2]]->z;

			if(triBoxOverlap(cent, boxhalfsize, triverts))
			{
				VoxelResult.push_back(*itrVoxel);
			}
		}

		//
		itrVoxel = VoxelResult.begin();
		for (; itrVoxel != VoxelResult.end(); itrVoxel++)
		{
			if (find(result.begin(), result.end(), *itrVoxel) == result.end())
			{
				result.push_back(*itrVoxel);
			}
		}
	}

	return result;
}

//Output voxels
bool CBVHVoxelIndex::WriteVoxels(string strPath)
{
	ptr_BVHNode pRoot = GetRoot();
	VoxelptrList voxels;
	pRoot->GetVoxels(voxels);
	//
	VoxelptrList::iterator itrVoxelptr = voxels.begin();
	for (;itrVoxelptr != voxels.end(); itrVoxelptr++)
	{
		Point3d centPt = (*itrVoxelptr)->GetCenter(dVoxelSize);

	}
}

//////////////////////////////////////////////////////////////////////////
//local Operations
BoundingBox CBVHVoxelIndex::GetBBox()
{
	return BBox;
}

//Fetch operations
ptr_CVoxel CBVHVoxelIndex::GetData()
{
	return VoxelData;
}

//Query operations
bool CBVHVoxelIndex::IsRoot()
{
	if (parent == nullptr)
	{
		return true;
	}
	else
		return false;
}

bool CBVHVoxelIndex::IsLeaf()
{
	if (children.size() == 0 && parent != nullptr)
	{
		return true;
	}
	else
		return false;
}
*/